/******************************************************************************/
/*  mbutils.c                                                                 */
/*                                                                            */
/*  FreeModbus Libary: A portable Modbus implementation for Modbus ASCII/RTU. */
/*  Copyright (c) 2006 Christian Walter <wolti@sil.at>                        */
/*  All rights reserved.                                                      */
/******************************************************************************/

#include "data_types.h"

#include "port\port.h"
#include "mb.h"
#include "mbproto.h"
#include "functions\mbutils.h"



#define BITS_UCHAR      (8U)



//==============================================================================
// xMbUtilSetBits()
//==============================================================================
void xMbUtilSetBits(uint8_t * _pByteBuf,
                    const uint16_t _bitOffset,
                    const uint8_t _numBits,
                    const uint8_t _value)
{
    uint16_t wordBuf;
    uint16_t mask;
    uint16_t byteOffset;
    uint16_t numPreBits;
    uint16_t value = _value;

    //assert(_numBits <= 8);
    //assert((size_t)BITS_UCHAR == sizeof(uint8_t) * 8);

    //  Calculate byte offset for first byte containing the bit values starting
    //  at _bitOffset.
    byteOffset = (uint16_t)((_bitOffset) / BITS_UCHAR);

    // How many bits precede our bits to set
    numPreBits = (uint16_t)(_bitOffset - byteOffset * BITS_UCHAR);

    // Move bit field into position over bits to set
    value <<= numPreBits;

    // Prepare a mask for setting the new bits
    mask = (uint16_t)((1 << (uint16_t) _numBits) - 1);
    mask <<= _bitOffset - byteOffset * BITS_UCHAR;

    // Copy bits into temporary storage
    wordBuf = _pByteBuf[byteOffset];
    wordBuf |= _pByteBuf[byteOffset + 1] << BITS_UCHAR;

    // Zero out bit field bits and then or value bits into them
    wordBuf = (uint16_t)((wordBuf & (~mask)) | value);

    // Move bits back into storage
    _pByteBuf[byteOffset] = (uint8_t)(wordBuf & 0xFF);
    _pByteBuf[byteOffset + 1] = (uint8_t)(wordBuf >> BITS_UCHAR);
}
//==============================================================================
//==============================================================================


//==============================================================================
// xMbUtilGetBits()
//==============================================================================
uint8_t xMbUtilGetBits(uint8_t * _pByteBuf,
                       const uint16_t _bitOffset,
                       const uint8_t _numBits)
{
    uint16_t wordBuf;
    uint16_t mask;
    uint16_t byteOffset;
    uint16_t numPreBits;

    // Calculate byte offset for first byte containing the bit values starting
    // at _bitOffset.
    byteOffset = (uint16_t)((_bitOffset) / BITS_UCHAR);

    // How many bits precede our bits to set
    numPreBits = (uint16_t)(_bitOffset - byteOffset * BITS_UCHAR);

    // Prepare a mask for setting the new bits
    mask = (uint16_t)((1 << (uint16_t) _numBits) - 1);

    // Copy bits into temporary storage
    wordBuf = _pByteBuf[byteOffset];
    wordBuf |= _pByteBuf[byteOffset + 1] << BITS_UCHAR;

    // Throw away unneeded bits
    wordBuf >>= numPreBits;

    // Mask away bits above the requested bitfield
    wordBuf &= mask;

    return (uint8_t)wordBuf;
}
//==============================================================================
//==============================================================================


//==============================================================================
// prveMbError2Exception()
//==============================================================================
eMB_EXCEPTION prveMbError2Exception(const eMB_ERROR_CODE _errorCode)
{
    eMB_EXCEPTION status;

    switch (_errorCode)
    {
        //----------------------------------------------------------------------
        case MB_ENOERR:
            status = MB_EX_NONE;
        break;

        //----------------------------------------------------------------------
        case MB_ENOREG:
            status = MB_EX_ILLEGAL_DATA_ADDRESS;
        break;

        //----------------------------------------------------------------------
        case MB_EINVAL:
            status = MB_EX_ILLEGAL_DATA_VALUE;
        break;

        //----------------------------------------------------------------------
        case MB_ACKNOWLEDGE:
            status = MB_EX_ACKNOWLEDGE;
        break;

        //----------------------------------------------------------------------
        case MB_ETIMEDOUT:
            status = MB_EX_SLAVE_BUSY;
        break;

        //----------------------------------------------------------------------
        default:
            status = MB_EX_SLAVE_DEVICE_FAILURE;
        break;
    };

    return status;
}
//==============================================================================
//==============================================================================